//
// Created by luzhan on 19-10-4.
//

#include "energy.hpp"
#include "timer.hpp"
using namespace cv;
using std::vector;
using std::cout;

//#define RED
Energy::Energy() {
    isFindCenterR = false;
    isFindTatgetArmor = false;
}

Energy::~Energy() {
}

bool Energy::findCurrentArmor(cv::Mat src) {
    Timer clock;
    this->isFindTatgetArmor = false;
    this->isFindCenterR = false;
    Mat bin_image;
    vector<Mat> channels(3);
    split(src, channels);
#ifdef RED
    threshold(channels[2] - channels[0], bin_image, 90, 255, THRESH_BINARY);
#else
    threshold(channels[0] - channels[2], bin_image, 90, 255, THRESH_BINARY);
#endif
    Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
    dilate(bin_image, bin_image, element, Point(-1, -1), 2);
    imshow("before",bin_image);
    floodFill(bin_image, Point(1, 1), Scalar(255), 0, FLOODFILL_FIXED_RANGE);

    imshow("after",bin_image);
    waitKey();
    element = getStructuringElement(MORPH_RECT, Size(5, 5));
    erode(bin_image, bin_image, element, Point(-1, -1), 3);
//        imshow("BIN", bin_image);
    vector<vector<Point>> contours;
    findContours(bin_image, contours, RETR_TREE, CHAIN_APPROX_NONE);
    for (size_t i = 0; i < contours.size(); ++i) {
        double area = contourArea(contours[i]);
        if (area > ENERGY_MAXAREA || area < ENERGY_MINAREA) continue;
        //面积符合 即可保留
        this->isFindTatgetArmor = true;
        this->target_contour = contours[i];
        this->target_energy_armor = minAreaRect(this->target_contour);
        this->target_x = this->target_energy_armor.center.x;
        this->target_y = this->target_energy_armor.center.y;
        break;
    }
    cout<<"找能量板    ";
    clock.stop();
    return this->isFindTatgetArmor;

}

bool Energy::findCenterR(cv::Mat src) {
    Timer clock;
//    更新样本点 计算拟合圆
    if (energy_armor_centers->size()<30) {
        energy_armor_centers->emplace_back(target_energy_armor.center);
    }else {
        this->isFindCenterR = calculate();
        //删除异常样本点
        if (!this->isFindCenterR)
            energy_armor_centers->pop_back();
        //更新样本点
        energy_armor_centers->erase(energy_armor_centers->begin());
        energy_armor_centers->emplace_back(target_energy_armor.center);

    }
    cout<<"找中心点R    ";
    clock.stop();
    return this->isFindCenterR;

}

//最小二乘法拟合圆，推导过程详见
//https://blog.csdn.net/Jacky_Ponder/article/details/70314919
bool Energy::calculate() {
//    if (energy_armor_centers->size()<30)
//    {
//        cout<<"样本点数量不足";
//        return false;
//    }
    Timer clock;
    int N = energy_armor_centers->size();
    double Xi, Yi;
    double sigmaX, sigmaY;
    double sigmaX2, sigmaXY, sigmaY2;
    double sigmaX3, sigmaX2Y, sigmaXY2, sigmaY3;

    size_t i;
    for (i = 0; i < energy_armor_centers->size(); ++i)
    {
        Xi = energy_armor_centers->at(i).x;
        Yi = energy_armor_centers->at(i).y;
        sigmaX += Xi;
        sigmaY += Yi;
        sigmaX2 += Xi * Xi;
        sigmaXY += Xi * Yi;
        sigmaY2 += Yi * Yi;
        sigmaX3 += Xi * Xi * Xi;
        sigmaX2Y += Xi * Xi * Yi;
        sigmaXY2 += Xi * Yi * Yi;
        sigmaY3 += Yi * Yi * Yi;
    }

    double C, D, E, G, H, a, b, c;
    C = N * sigmaX2 - sigmaX * sigmaX;
    D = N * sigmaXY - sigmaX * sigmaY;
    E = N * (sigmaX3 + sigmaXY2) - sigmaX * (sigmaX2 + sigmaY2);
    G = N * sigmaY2 - sigmaY * sigmaY;
    H = N * (sigmaY3 + sigmaX2Y) - sigmaY * (sigmaX2 + sigmaY2);
    a = (H*D - E*G) / (C*G - D*D);
    b = (H*C - E*D) / (D*D - C*G);
    if (isnan(a) || isnan(b))
        return false;
    c = -(sigmaX2 + sigmaY2 + a*sigmaX + b*sigmaY) / N;
    this->circle_center.x = a / (-2);
    this->circle_center.y = b / (-2);


    this->circle_radius = sqrt(a*a + b*b -4*c) / 2;
    if (isnan(circle_radius))
        return false;
//    cout<<circle_radius<<'\n';
    cout<<"计算拟合圆    ";
    clock.stop();
    if (this->circle_radius < MINRADIUS || this->circle_radius > MAXRADIUS)
        return false;
    else
        return true;
}